﻿using ModbusCommunication;
using System;
using System.Collections.Generic;
using System.Net;
using System.Net.Sockets;
using System.Text;
using System.Threading;

namespace cutter
{
    class UR3Control
    {

        public Socket ClientSocket;
        //public IPAddress myIP;
        public int TargetPORT;
        //public IPEndPoint ipe;
        public ModbusSocket Socket502_TCP;

        public static double SpeedRate = 0.12;
        public static double AccelerationRate = 0.2;

        public static string PosStr(float[] p)
        {
            string str;

            str = "(p[" + p[0].ToString("0.0000") + "," + p[1].ToString("0.0000") + "," + p[2].ToString("0.0000") + ","
                + p[3].ToString("0.0000") + "," + p[4].ToString("0.0000") + "," + p[5].ToString("0.0000") + "], a = "
                + AccelerationRate.ToString() + ", v = " + SpeedRate.ToString() + ")";

            return str;
        }


        public UR3Control(string Target_IP = "192.168.10.88")
        {
            Creat_client(Target_IP);
            Console.WriteLine("Creat_client");
        }


        //创建只需要实例化这个socket即可，不需要连接
        //初始化URControlHandle，生成一个clientSocket，方便从30001-30003端口直接发送指令
        void Creat_client(string IP, int PORT = 30003)
        {
            //所有UR的控制都在这里完成，先生成一个ClientSocket
            ClientSocket = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.Tcp);
            IPAddress myIP = IPAddress.Parse(IP);
            TargetPORT = PORT;
            IPEndPoint ipe = new IPEndPoint(myIP, TargetPORT);

            //一次连接终生使用
            try
            {
                ClientSocket.Connect(ipe);
            }
            catch (Exception connectError)
            {
                Console.WriteLine(connectError.ToString());
            }

            Socket502_TCP = new ModbusSocket();
            //初始化这个Socket502_TCP端口
            Socket502_TCP.RemoteIP = IP;
            Socket502_TCP.RemotePort = 502;
            Socket502_TCP.SocketTimeOut = 1000;
            Socket502_TCP.initialServer();
        }

        //因为连接是同步的方法，会导致阻塞，所以把连接功能放到与发送一起执行
        void Send_command(string command)
        {
            command += "\r\n";
            byte[] buffersend = System.Text.Encoding.Default.GetBytes(command);

            try
            {
                ClientSocket.Send(buffersend);
            }
            catch (Exception sendError)
            {
                Console.WriteLine(sendError.ToString());
                //这就是说明没有连接到UR(实际操作中，即便连接正常了，也会有这样的问题)
                Console.WriteLine("未取得与UR的连接，请确认连接正常。");
            }
        }

        string Feedback()
        {
            byte[] bytes = new byte[1024];
            string data = "";
            int bytesRec = ClientSocket.Receive(bytes);

            data += Encoding.ASCII.GetString(bytes, 0, bytesRec);
            return data;
        }

        //前面发送的指令都是单向的，但是有的指令是请求UR提供返回值的
        string Send_command_WithFeedback(string command)
        {
            Send_command(command);
            return Feedback();
        }

        //只有在点击退出按钮才关闭
        public void Close_client()
        {
            ClientSocket.Shutdown(SocketShutdown.Both);
            ClientSocket.Close();
        }

        public static float DiffNorm2(float x1, float y1, float z1, float x2 = 0, float y2 = 0, float z2 = 0)
        {
            return (x1 - x2) * (x1 - x2) + (y1 - y2) * (y1 - y2) + (z1 - z2) * (z1 - z2);
        }

        public static bool ComparePos(float[] p1, float[] p2, float threshold, float thresholdR)
        {
            float d2xyz = DiffNorm2(p1[0], p1[1], p1[2], p2[0], p2[1], p2[2]);

            if (d2xyz > threshold * threshold)
                return false;

            float d2rxyz = DiffNorm2(p1[3], p1[4], p1[5], p2[3], p2[4], p2[5]);

            if (d2rxyz <= thresholdR * thresholdR)
                return true;

            double Arxyz = Math.Sqrt((double)DiffNorm2(p1[3], p1[4], p1[5]));

            float scale = (float)((Arxyz - 2 * Math.PI) / Arxyz);

            d2rxyz = DiffNorm2(p1[3] * scale, p1[4] * scale, p1[5] * scale, p2[3], p2[4], p2[5]);

            return (d2rxyz <= thresholdR * thresholdR);
        }

        public float[] GetTCP()
        {
            //这里实时读取UR数据（先读取六个坐标值，手册来看地址就是400）
            int[] SixPositions = Socket502_TCP.ReadMultipleRegister(6, 400, false);
            for (int i = 0; i < 6; i++)
            {
                //见Modbus UR参考文档：如果XYZ有超过32768，则变成负值（虽然是0-65535，但是我们要自己变成有符号数“-32767”到“32768”）
                if (SixPositions[i] > 32768)
                {
                    SixPositions[i] -= 65535;
                }
            }
            //Console.WriteLine("get: " + SixPositions[0] + "\t" + SixPositions[1] + "\t" + SixPositions[2] + "\t" + SixPositions[3] + "\t" + SixPositions[4] + "\t" + SixPositions[5]);
            //对于XYZ，取得的并不能直接使用，而是要经过转换
            float[] PositionFiltered = new float[] {
                (float)SixPositions[0] / 10000, (float)SixPositions[1] / 10000, (float)SixPositions[2] / 10000,
                (float)SixPositions[3] / 1000,  (float)SixPositions[4] / 1000,  (float)SixPositions[5] / 1000 };
            //Console.WriteLine("get: " + PositionFiltered[0] + "\t" + PositionFiltered[1] + "\t" + PositionFiltered[2] + "\t" + PositionFiltered[3] + "\t" + PositionFiltered[4] + "\t" + PositionFiltered[5]);
            return PositionFiltered;
        }

        public float[] GetTCP(int n)
        {
            float[] tcp = GetTCP();
            if (n > 1)
            {
                for (int i = 1; i < n; i++)
                {
                    float[] tmp = GetTCP();
                    for (int j = 0; j < tcp.Length; j++)
                        tcp[j] += tmp[j];
                    Thread.Sleep(50);
                }
                for (int j = 0; j < tcp.Length; j++)
                    tcp[j] /= n;
            }
            return tcp;
        }

        bool IgnoreProtectiveStop()
        {
            //这里实时读取UR数据（读取是否开机，是否紧急停机等状态信息，手册来看地址就是260）
            //260 x x x* isPowerOnRobot 
            //261 x x x* isSecurityStopped   
            //262 x x x* isEmergencyStopped 
            //263 x x x* isTeachButtonPressed 
            //264 x x x* isPowerPuttonPressed  
            //265 x x x* isSafetySignalSuchThatWeShouldStop 
            int[] RobotState = Socket502_TCP.ReadMultipleRegister(6, 260, false);

            if (RobotState[1] != 0)
            {
                Console.WriteLine("protective stop");
                Send_command_WithFeedback("unlock protective stop");
                return true;
            }
            return false;
        }

        public void GoToPos(float[] p, string moveCmd = "movel")
        {
            Send_command(moveCmd + PosStr(p));
            while (!ComparePos(GetTCP(), p, 0.0005f, 0.002f))
            {
                //if (IgnoreProtectiveStop()) return;
                Thread.Sleep(100);
            }
        }

        public void MovePath(ref List<float> p, bool manual = false, string moveCmd = "movel")
        {
            for (int i = 0; i + 5 < p.Count; i += 6)
            {
                float[] pos = new float[] { p[i], p[i + 1], p[i + 2], p[i + 3], p[i + 4], p[i + 5] };

                GoToPos(pos, moveCmd);

                Console.WriteLine("GoToPos " + i / 6);

                if (manual)
                {
                    Console.WriteLine("press enter to continue");
                    string line = Console.ReadLine();
                }
            }
        }

        public void PowerOn()
        {
            Send_command_WithFeedback("power on");
            Thread.Sleep(3000);
            Send_command_WithFeedback("brake release");
        }

        public void PowerOff()
        {
            Send_command_WithFeedback("power off");
        }

    }
}
